/*
* 电机指令列表
 * 'o' 利用currentPosition获取当前电机输出轴位置并通过串口监视器输出该信息
 * 'v' 利用moveTo函数使电机运行到用户指定坐标位置，moveTo函数不会“block”程序。
 *     例：v1024 - 将电机moveTo到1024位置
 * 'm' 利用move函数使电机运行相应步数。move函数不会“block”程序。
 *     例：m1024 - 使电机运行1024步
 * 'r' 利用runToNewPosition函数让电机运行到用户指定坐标位置。runToNewPosition函数功能
 *     与moveTo函数功能基本相同。唯一区别是runToNewPosition函数将“block”程序运行。
 *     即电机没有到达目标位置前，Arduino将不会继续执行后续程序内容。
 *     这一点很像Arduino官方Stepper库中的step函数。
 *     例：r1024 - 将电机runToNewPosition到1024位置。Arduino在电机达到1024位置前将停止
 *     其它程序任务的执行。
 * 's' 利用setCurrentPosition函数设置当前位置为用户指定位置值。
 *     例：s0 - 设置当前位置为0
 * 'a' 利用setAcceleration函数设置加速度
 *     例：a100 - 设置电机运行的加速度为100
 * 'x' 利用setMaxSpeed函数设置最大速度
 *     例：x500 - 设置电机运行的最大速度为500
 * 'd' 用户通过此指令可指定哪一个电机进行工作
 *     例：d1 一号电机工作，d2 二号电机工作, d3 三号电机工作,d0 4个电机同时工作
*/
#include <AccelStepper.h>  //本示例程序使用AccelStepper库
#include <Servo.h>
//
// 定义电机控制用常量

// A4988连接Arduino引脚号
const int xdirPin = 2;     // 方向控制引脚
const int xstepPin = 6;    // 步进控制引脚
const int enablePin = 8;  // 使能控制引脚
const int ydirPin = 6;     // y方向控制引脚
const int ystepPin = 3;    // y步进控制引脚
const int zdirPin = 7;     // z方向控制引脚
const int zstepPin = 4;    // z步进控制引脚

const int moveSteps = 1000;    //运行步数

int pos = 0;    // 存储伺服电机角度信息的变量

char cmd;        //电机指令字符
int data;        //电机指令参数
int stepperNum;  //控制电机编号

AccelStepper stepper1(1,xstepPin,xdirPin);//建立步进电机对象
AccelStepper stepper2(1,ystepPin,ydirPin);//建立步进电机对象2
AccelStepper stepper3(1,zstepPin,zdirPin);//建立步进电机对象3

Servo myservo;

void setup() {
  Serial.begin(9600);

   pinMode(xstepPin,OUTPUT);     // Arduino控制A4988x步进引脚为输出模式
  pinMode(xdirPin,OUTPUT);      // Arduino控制A4988x方向引脚为输出模式
  pinMode(ystepPin,OUTPUT);     // Arduino控制A4988y步进引脚为输出模式
  pinMode(ydirPin,OUTPUT);      // Arduino控制A4988y方向引脚为输出模式
  pinMode(zstepPin,OUTPUT);     // Arduino控制A4988z步进引脚为输出模式
  pinMode(zdirPin,OUTPUT);      // Arduino控制A4988z方向引脚为输出模式
  pinMode(enablePin,OUTPUT);   // Arduino控制A4988使能引脚为输出模式
  digitalWrite(enablePin,LOW); // 将使能控制引脚设置为低电平从而让
                                // 电机驱动板进入工作状态

  stepper1.setMaxSpeed(1000.0);     // 设置电机最大速度1000
  stepper1.setAcceleration(100.0);  // 设置电机加速度100.0
 stepper2.setMaxSpeed(300.0);     // 设置电机最大速度300
  stepper2.setAcceleration(20.0);  // 设置电机加速度20.0
  stepper3.setMaxSpeed(300.0);     // 设置电机最大速度300
  stepper3.setAcceleration(20.0);  // 设置电机加速度20.0

  myservo.attach(11);  // Servo对象连接在9号引脚
}

void loop() {
  if (Serial.available()) {     // 检查串口缓存是否有数据等待传输
    cmd = Serial.read();        // 获取电机指令中指令信息
    Serial.print(F("cmd = "));
    Serial.print(cmd);
    Serial.print(F(" , "));

    data = Serial.parseInt();   // 获取电机指令中参数信息
    Serial.print(F("data = "));
    Serial.print(data);
    Serial.println(F(""));

    runUsrCmd();
  }



  stepper1.run();   // 1号电机运行
 stepper2.run();   // 2号电机运行
  stepper3.run();   // 3号电机运行
}

void runUsrCmd(){
  switch(cmd){
    case 'o': //利用currentPosition获取当前电机输出轴位置并通过串口监视器输出该信息

      Serial.print(F("stepper1 Position: "));
      Serial.println(stepper1.currentPosition());
      Serial.print(F("stepper2 Position: "));
      Serial.println(stepper2.currentPosition());
      Serial.print(F("stepper3 Position: "));
      Serial.println(stepper3.currentPosition());
      Serial.print(F("Current Running Motor: "));

      if (stepperNum == 1 || stepperNum == 2 || stepperNum == 3) {
        Serial.print(F("Motor# "));
        Serial.println(stepperNum);
      } else if (stepperNum == 0 ) {
        Serial.println(F("4 Motors"));
      }
      break;

    case 'v': //利用moveTo函数使电机运行到用户指定坐标位置，moveTo函数不会“block”程序。

      if (stepperNum == 1){
        Serial.print(F("Motor1 'moveTo' "));  Serial.println(data);
        stepper1.moveTo(data);
      } else if (stepperNum == 2){
        Serial.print(F("Motor2 'moveTo' "));  Serial.println(data);
        stepper2.moveTo(data);
      }else if (stepperNum == 3){
        Serial.print(F("Motor3 'moveTo' "));  Serial.println(data);
        stepper2.moveTo(data);
      }else if (stepperNum == 0){
        Serial.print(F("4 Motors 'moveTo' "));  Serial.println(data);
        stepper1.moveTo(data);
        stepper2.moveTo(data);
        stepper3.moveTo(data);
      }
      break;

    case 'm':   // 利用move函数使电机运行相应步数。move函数不会“block”程序。

      if (stepperNum == 1){
        Serial.print(F("Motor1 'move'  "));  Serial.println(data);
        stepper1.move(data);
      } else if (stepperNum == 2){
        Serial.print(F("Motor2 'move'  "));  Serial.println(data);
        stepper2.move(data);
      }else if (stepperNum == 3){
        Serial.print(F("Motor3 'move'  "));  Serial.println(data);
        stepper2.move(data);
      }else if (stepperNum == 0){
        Serial.print(F("4 Motors 'move'  "));  Serial.println(data);
        stepper1.move(data);
        stepper2.move(data);
      }
      break;

    case 'r':  //利用runToNewPosition函数让电机运行到用户指定位置值。
      if (stepperNum == 1){
        Serial.print(F("Motor1 'runToNewPosition' "));
        Serial.println(data);
        stepper1.runToNewPosition(data);
      } else if (stepperNum == 2){
        Serial.print(F("Motor2 'runToNewPosition' "));
        Serial.println(data);
        stepper2.runToNewPosition(data);
      }else if (stepperNum == 3){
        Serial.print(F("Motor3 'runToNewPosition' "));
        Serial.println(data);
        stepper3.runToNewPosition(data);
      }else if (stepperNum == 0){
        Serial.print(F("Both Motors 'runToNewPosition' "));
        Serial.println(data);
        stepper1.runToNewPosition(data);
        stepper2.runToNewPosition(data);
      }
      break;

    case 's':   //利用setCurrentPosition函数设置当前位置为用户指定位置值。
      if (stepperNum == 1){
        Serial.print(F("Set stepper1 Current Position to "));
        Serial.println(data);
        stepper1.setCurrentPosition(data);
      } else if (stepperNum == 2){
        Serial.print(F("Set stepper2 Current Position to "));
        Serial.println(data);
        stepper2.setCurrentPosition(data);
      }else if (stepperNum == 3){
        Serial.print(F("Set stepper3 Current Position to "));
        Serial.println(data);
        stepper3.setCurrentPosition(data);
      }else if (stepperNum == 0){
        stepper1.moveTo(data);
        Serial.print(F("Set both steppers' Current Position to "));
        Serial.println(data);
        stepper1.setCurrentPosition(data);
        stepper2.setCurrentPosition(data);
      }
      break;

    case 'a':  //利用setAcceleration函数设置加速度
      if (stepperNum == 1){
        Serial.print(F("Motor1 'setAcceleration' "));
        Serial.println(data);
        stepper1.setAcceleration(data);
      } else if (stepperNum == 2){
        Serial.print(F("Motor2 'setAcceleration' "));
        Serial.println(data);
        stepper2.setAcceleration(data);
      } else if (stepperNum == 3){
        Serial.print(F("Motor3 'setAcceleration' "));
        Serial.println(data);
        stepper3.setAcceleration(data);
      }else if (stepperNum == 0){
        Serial.print(F("Both Motors 'setAcceleration' "));
        Serial.println(data);
        stepper1.setAcceleration(data);
        stepper2.setAcceleration(data);
        stepper3.setAcceleration(data);
      }
      break;

    case 'x':  //利用setMaxSpeed函数设置最大速度
      if (stepperNum == 1){
        Serial.print(F("Motor1 'setMaxSpeed' "));
        Serial.println(data);
        stepper1.setMaxSpeed(data);
      } else if (stepperNum == 2){
        Serial.print(F("Motor2 'setMaxSpeed' "));
        Serial.println(data);
        stepper2.setMaxSpeed(data);
      }else if (stepperNum == 3){
        Serial.print(F("Motor3 'setMaxSpeed' "));
        Serial.println(data);
        stepper3.setMaxSpeed(data);
      }else if (stepperNum == 0){
        Serial.print(F("4 Motors 'setMaxSpeed' "));
        Serial.println(data);
        stepper1.setMaxSpeed(data);
        stepper2.setMaxSpeed(data);
        stepper3.setMaxSpeed(data);
      }
      break;

    case 'd':  //用户通过此指令可指定哪一个电机进行工作
      if ( data == 1 || data == 2 || data == 3){
        stepperNum = data;
        Serial.print(F("Running Motor "));  Serial.println(stepperNum);
      } else if (data == 0 ){
        stepperNum = data;
        Serial.println(F("Running 4 Motors "));
      } else {
        Serial.print(F("Motor Number Wrong."));
      }
      break;

    default:  // 未知指令
      Serial.println(F("Unknown Command"));
  }
}

